/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.templates.RobotMap;

/**
 *
 * @author Developer
 */
public class driveAim extends PIDSubsystem {

    private static final double Kp = 0.6;
    private static final double Ki = 1.0;
    private static final double Kd = 0.5;

    // Initialize your subsystem here
    NetworkTable xtable;
    public driveAim() {
        super("driveAim", Kp, Ki, Kd);

        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.
        setAbsoluteTolerance(1);
        getPIDController().setContinuous(false);
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }

    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;
        xtable = NetworkTable.getTable("robo");
        return xtable.getNumber("x");
    }

    protected void usePIDOutput(double output) {
        System.out.println("out val:" + output);
        
        RobotMap.frontLeftMotor1.set(output);
        RobotMap.frontRightMotor1.set(output);
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
    }
}
